/**
 * @ Author: luoqi
 * @ Create Time: 2025-02-10 16:13
 * @ Modified by: luoqi
 * @ Modified time: 2025-02-11 23:03
 * @ Description:
 */

#include "trajc_plan.h"

int trajc_init(TrajcSPlan *trajc, qfp_t jerk_max, qfp_t acc_max, qfp_t vel_max)
{
    if(!trajc || jerk_max <= 0 || acc_max <= 0 || vel_max <= 0) {
        return -1;
    }

    trajc->jerk_max = jerk_max;
    trajc->acc_max = acc_max;
    trajc->vel_max = vel_max;

    trajc->x0 = 0;
    trajc->xt = 0;
    trajc->v0 = 0;
    trajc->vt = 0;
    trajc->t_ploop = 0;
    trajc->t_vloop = 0;
    trajc->t_aloop = 0;
    
    trajc->t1 = 0;
    trajc->t2 = 0;
    trajc->t3 = 0;
    trajc->t4 = 0;
    trajc->t5 = 0;
    trajc->t6 = 0;
    trajc->t7 = 0;
    trajc->t_total = 0;
    
    return 0;
}

int trajc_update(TrajcSPlan *trajc, qfp_t x0, qfp_t xt, qfp_t v0, qfp_t vt)
{
    if(!trajc) {
        return -1;
    }
    trajc->v0 = v0;
    trajc->vt = vt;
    trajc->x0 = x0;
    trajc->xt = xt;
    
    trajc->t_ploop = 0;
    trajc->t_vloop = 0;
    trajc->t_aloop = 0;
    qfp_t t_total = 0;

    trajc->t1 = trajc->acc_max / trajc->jerk_max;


    return 0;
}

qfp_t trajc_pos_calc(TrajcSPlan *trajc, qfp_t dt)
{
    if(!trajc) {
        return 0;
    }

    qfp_t p_k = 0;

    trajc->t_ploop += dt;


    return p_k;
}

qfp_t trajc_vel_calc(TrajcSPlan *trajc, qfp_t dt)
{
    if(!trajc) {
        return -1;
    }

    qfp_t v_k = 0;

    trajc->t_vloop += dt;

    return v_k;
}

qfp_t trajc_acc_calc(TrajcSPlan *trajc, qfp_t dt)
{
    if(!trajc) {
        return -1;
    }

    qfp_t a_k = 0;

    trajc->t_aloop += dt;

    return a_k;
}
